#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include "config.h"
#include "ultrasonic_sensor.h"

// 超声波传感器配置
#define TRIG_PIN 7
#define ECHO_PIN 8
#define MAX_DISTANCE 400 // 最大检测距离(cm)
#define TIMEOUT_US 23000 // 超时时间(微秒)
#define SOUND_SPEED 343  // 声速(m/s)

// 传感器状态
static ultrasonic_state_t sensor_state = ULTRASONIC_IDLE;
static float last_distance = 0.0f;
static uint32_t last_measurement_time = 0;
static bool sensor_initialized = false;

// 滤波相关
static float distance_buffer[DISTANCE_BUFFER_SIZE];
static int buffer_index = 0;
static bool buffer_full = false;

// 私有函数声明
static void gpio_init(void);
static void timer_init(void);
static uint32_t get_microseconds(void);
static void delay_microseconds(uint32_t us);
static float calculate_distance(uint32_t pulse_width_us);
static float median_filter(float new_value);
static float moving_average_filter(float new_value);
static bool is_distance_valid(float distance);

/**
 * 初始化超声波传感器
 */
int ultrasonic_sensor_init(void)
{
    DEBUG_PRINT(2, "初始化超声波传感器...");

    // 初始化GPIO
    gpio_init();

    // 初始化定时器
    timer_init();

    // 初始化滤波缓冲区
    memset(distance_buffer, 0, sizeof(distance_buffer));
    buffer_index = 0;
    buffer_full = false;

    sensor_state = ULTRASONIC_IDLE;
    last_distance = 0.0f;
    sensor_initialized = true;

    DEBUG_PRINT(2, "超声波传感器初始化完成");

    return 0;
}

/**
 * 测量距离
 */
float ultrasonic_measure_distance(void)
{
    if (!sensor_initialized)
    {
        DEBUG_PRINT(0, "超声波传感器未初始化");
        return -1.0f;
    }

    if (sensor_state != ULTRASONIC_IDLE)
    {
        DEBUG_PRINT(1, "传感器忙碌中");
        return last_distance;
    }

    sensor_state = ULTRASONIC_MEASURING;

    // 1. 发送触发脉冲
    // 设置Trig引脚为高电平10微秒
    // TODO: 实际GPIO操作
    // gpio_set_high(TRIG_PIN);
    delay_microseconds(10);
    // gpio_set_low(TRIG_PIN);

    // 2. 等待Echo引脚变为高电平
    uint32_t start_time = get_microseconds();
    uint32_t timeout = start_time + TIMEOUT_US;

    // 等待Echo引脚变高
    while (get_microseconds() < timeout)
    {
        // TODO: 检查Echo引脚状态
        // if (gpio_read(ECHO_PIN) == HIGH) break;
        break; // 模拟检测到高电平
    }

    if (get_microseconds() >= timeout)
    {
        DEBUG_PRINT(1, "超声波传感器超时");
        sensor_state = ULTRASONIC_IDLE;
        return -1.0f;
    }

    // 3. 测量Echo引脚高电平持续时间
    uint32_t pulse_start = get_microseconds();
    timeout = pulse_start + TIMEOUT_US;

    // 等待Echo引脚变低
    while (get_microseconds() < timeout)
    {
        // TODO: 检查Echo引脚状态
        // if (gpio_read(ECHO_PIN) == LOW) break;
        delay_microseconds(100); // 模拟测量时间
        break;
    }

    uint32_t pulse_end = get_microseconds();
    uint32_t pulse_width = pulse_end - pulse_start;

    // 4. 计算距离
    float raw_distance = calculate_distance(pulse_width);

    // 5. 验证距离有效性
    if (!is_distance_valid(raw_distance))
    {
        DEBUG_PRINT(1, "距离测量无效: %.2f cm", raw_distance);
        sensor_state = ULTRASONIC_IDLE;
        return -1.0f;
    }

    // 6. 应用滤波
    float filtered_distance = median_filter(raw_distance);
    filtered_distance = moving_average_filter(filtered_distance);

    last_distance = filtered_distance;
    last_measurement_time = get_microseconds();
    sensor_state = ULTRASONIC_IDLE;

    DEBUG_PRINT(3, "距离测量: 原始=%.2f cm, 滤波后=%.2f cm", raw_distance, filtered_distance);

    return filtered_distance;
}

/**
 * 获取最后一次测量的距离
 */
float ultrasonic_get_last_distance(void)
{
    return last_distance;
}

/**
 * 获取传感器状态
 */
ultrasonic_state_t ultrasonic_get_state(void)
{
    return sensor_state;
}

/**
 * 检查传感器是否就绪
 */
bool ultrasonic_is_ready(void)
{
    return sensor_initialized && (sensor_state == ULTRASONIC_IDLE);
}

/**
 * 获取最后测量时间
 */
uint32_t ultrasonic_get_last_measurement_time(void)
{
    return last_measurement_time;
}

/**
 * 重置传感器
 */
int ultrasonic_reset(void)
{
    DEBUG_PRINT(2, "重置超声波传感器");

    sensor_state = ULTRASONIC_IDLE;
    last_distance = 0.0f;

    // 清空滤波缓冲区
    memset(distance_buffer, 0, sizeof(distance_buffer));
    buffer_index = 0;
    buffer_full = false;

    return 0;
}

/**
 * 设置测量参数
 */
int ultrasonic_set_params(uint32_t timeout_us, float max_distance)
{
    if (timeout_us > 50000 || max_distance > 500.0f)
    {
        DEBUG_PRINT(0, "参数超出范围");
        return -1;
    }

    DEBUG_PRINT(2, "设置测量参数: 超时=%d us, 最大距离=%.1f cm", timeout_us, max_distance);

    // TODO: 更新参数

    return 0;
}

/**
 * 获取传感器信息
 */
void ultrasonic_get_info(ultrasonic_info_t *info)
{
    if (!info)
        return;

    info->state = sensor_state;
    info->last_distance = last_distance;
    info->last_measurement_time = last_measurement_time;
    info->is_initialized = sensor_initialized;
    info->max_distance = MAX_DISTANCE;
    info->min_distance = 2.0f; // HC-SR04最小检测距离
}

// 私有函数实现

/**
 * 初始化GPIO
 */
static void gpio_init(void)
{
    // TODO: 根据EBP3901的具体GPIO配置
    // 配置Trig引脚为输出
    // 配置Echo引脚为输入

    DEBUG_PRINT(3, "GPIO初始化完成");
}

/**
 * 初始化定时器
 */
static void timer_init(void)
{
    // TODO: 配置高精度定时器用于微秒级延时和计时

    DEBUG_PRINT(3, "定时器初始化完成");
}

/**
 * 获取微秒时间戳
 */
static uint32_t get_microseconds(void)
{
    // TODO: 实际获取微秒时间戳
    // 这里模拟返回递增的时间戳
    static uint32_t simulated_time = 0;
    simulated_time += 1000; // 每次调用增加1ms
    return simulated_time;
}

/**
 * 微秒级延时
 */
static void delay_microseconds(uint32_t us)
{
    // TODO: 实际的微秒延时
    // 这里使用简单的循环延时（不准确，仅用于演示）
    volatile uint32_t count = us * 10; // 假设的循环次数
    while (count--)
    {
        // 空循环
    }
}

/**
 * 计算距离
 */
static float calculate_distance(uint32_t pulse_width_us)
{
    // 距离 = (脉冲宽度 × 声速) / 2
    // 声速 = 343 m/s = 0.0343 cm/us
    float distance = (pulse_width_us * 0.0343f) / 2.0f;

    return distance;
}

/**
 * 中值滤波
 */
static float median_filter(float new_value)
{
    // 简化的中值滤波，使用3个值
    static float values[3] = {0};
    static int index = 0;

    values[index] = new_value;
    index = (index + 1) % 3;

    // 排序并返回中值
    float sorted[3];
    memcpy(sorted, values, sizeof(values));

    // 简单的冒泡排序
    for (int i = 0; i < 2; i++)
    {
        for (int j = 0; j < 2 - i; j++)
        {
            if (sorted[j] > sorted[j + 1])
            {
                float temp = sorted[j];
                sorted[j] = sorted[j + 1];
                sorted[j + 1] = temp;
            }
        }
    }

    return sorted[1]; // 返回中值
}

/**
 * 移动平均滤波
 */
static float moving_average_filter(float new_value)
{
    // 添加新值到缓冲区
    distance_buffer[buffer_index] = new_value;
    buffer_index = (buffer_index + 1) % DISTANCE_BUFFER_SIZE;

    if (!buffer_full && buffer_index == 0)
    {
        buffer_full = true;
    }

    // 计算平均值
    float sum = 0.0f;
    int count = buffer_full ? DISTANCE_BUFFER_SIZE : buffer_index;

    for (int i = 0; i < count; i++)
    {
        sum += distance_buffer[i];
    }

    return sum / count;
}

/**
 * 验证距离有效性
 */
static bool is_distance_valid(float distance)
{
    // 检查距离是否在有效范围内
    if (distance < 2.0f || distance > MAX_DISTANCE)
    {
        return false;
    }

    // 检查距离变化是否过于剧烈
    if (last_distance > 0.0f)
    {
        float change = distance - last_distance;
        if (change > 50.0f || change < -50.0f)
        {
            DEBUG_PRINT(1, "距离变化过大: %.2f -> %.2f cm", last_distance, distance);
            return false;
        }
    }

    return true;
}